#ifndef STATICPOINTMINIMIZEFUN_H_
#define STATICPOINTMINIMIZEFUN_H_

#include <cmath>
#include "precom.h"

class StaticPointMinimizeFun
{
public:
	StaticPointMinimizeFun(const MATHLIB::matrix33& a_surfNormals,
		const MATHLIB::matrix33& a_toolNormals)
		: m_surfN(a_surfNormals), m_toolN(a_toolNormals)
	{}
	StaticPointMinimizeFun() 
	{
		m_surfN.identity(); 
		m_toolN.identity();
	}
	~StaticPointMinimizeFun() {}

	void Init(const MATHLIB::matrix33& a_surfNormals,
		const MATHLIB::matrix33& a_toolNormals)
	{
		m_surfN = a_surfNormals;
		m_toolN = a_toolNormals;
	}

	inline MATHLIB::vector3 LinInterpolate(double a, double b, 
		const MATHLIB::matrix33& vecs) const
	{
		return vecs[0] * a + vecs[1] * b + vecs[2] * (1.0-a-b);
	}

	inline MATHLIB::vector3 fun(double a, double b) const
	{
		MATHLIB::vector3 v1, v2;
		v1 = LinInterpolate(a, b, m_surfN);
		v2 = LinInterpolate(a, b, m_toolN);
		return MATHLIB::CrossProduct(v1, v2);
	}

	inline double dxfun1(double a, double b) const
	{
		return (-m_surfN[2].z *m_toolN[0].y + m_surfN[2].y *m_toolN[0].z - m_surfN[0].z *m_toolN[2].y + 2 *m_surfN[2].z *m_toolN[2].y 
			+ 2 *a *(m_surfN[2].z *(m_toolN[0].y - m_toolN[2].y) + m_surfN[0].z *(-m_toolN[0].y + m_toolN[2].y) + (m_surfN[0].y 
			- m_surfN[2].y) *(m_toolN[0].z - m_toolN[2].z)) + m_surfN[0].y *m_toolN[2].z - 2 *m_surfN[2].y *m_toolN[2].z 
			+ b *(m_surfN[1].y *m_toolN[0].z - m_surfN[2].y *m_toolN[0].z - m_surfN[0].z *m_toolN[1].y + m_surfN[0].y *m_toolN[1].z 
			- m_surfN[2].y *m_toolN[1].z + m_surfN[2].z *(m_toolN[0].y + m_toolN[1].y - 2 *m_toolN[2].y) 
			+ m_surfN[0].z *m_toolN[2].y + m_surfN[1].z *(-m_toolN[0].y + m_toolN[2].y) - m_surfN[0].y *m_toolN[2].z 
			- m_surfN[1].y *m_toolN[2].z + 2 *m_surfN[2].y *m_toolN[2].z));
	}

	inline double dyfun1(double a, double b) const
	{
		return (-m_surfN[2].z *m_toolN[1].y + m_surfN[2].y *m_toolN[1].z - m_surfN[1].z *m_toolN[2].y + 2 *m_surfN[2].z *m_toolN[2].y + 
			2 *b *(m_surfN[2].z *(m_toolN[1].y - m_toolN[2].y) + m_surfN[1].z *(-m_toolN[1].y + m_toolN[2].y) + (m_surfN[1].y 
			- m_surfN[2].y) *(m_toolN[1].z - m_toolN[2].z)) + m_surfN[1].y *m_toolN[2].z - 2 *m_surfN[2].y *m_toolN[2].z 
			+ a *(m_surfN[1].y *m_toolN[0].z - m_surfN[2].y *m_toolN[0].z - m_surfN[0].z *m_toolN[1].y + m_surfN[0].y *m_toolN[1].z 
			- m_surfN[2].y *m_toolN[1].z + m_surfN[2].z *(m_toolN[0].y + m_toolN[1].y - 2 *m_toolN[2].y) + m_surfN[0].z *m_toolN[2].y 
			+ m_surfN[1].z *(-m_toolN[0].y + m_toolN[2].y) - m_surfN[0].y *m_toolN[2].z - m_surfN[1].y *m_toolN[2].z 
			+ 2 *m_surfN[2].y *m_toolN[2].z));
	}

	inline double dxfun2(double a, double b) const
	{
		return (m_surfN[2].z *m_toolN[0].x - m_surfN[2].x *m_toolN[0].z + m_surfN[0].z *m_toolN[2].x - 2 *m_surfN[2].z *m_toolN[2].x 
			+ 2 *a *(m_surfN[0].z *(m_toolN[0].x - m_toolN[2].x) + m_surfN[2].z *(-m_toolN[0].x + m_toolN[2].x) 
			- (m_surfN[0].x - m_surfN[2].x) *(m_toolN[0].z - m_toolN[2].z)) - m_surfN[0].x *m_toolN[2].z + 2 *m_surfN[2].x *m_toolN[2].z 
			+ b *(-m_surfN[1].x *m_toolN[0].z + m_surfN[2].x *m_toolN[0].z + m_surfN[0].z *m_toolN[1].x - m_surfN[0].x *m_toolN[1].z 
			+ m_surfN[2].x *m_toolN[1].z - m_surfN[2].z *(m_toolN[0].x + m_toolN[1].x - 2 *m_toolN[2].x) 
			+ m_surfN[1].z *(m_toolN[0].x - m_toolN[2].x) - m_surfN[0].z *m_toolN[2].x + m_surfN[0].x *m_toolN[2].z 
			+ m_surfN[1].x *m_toolN[2].z - 2 *m_surfN[2].x *m_toolN[2].z));
	}

	inline double dyfun2(double a, double b) const
	{
		return (m_surfN[2].z *m_toolN[1].x - m_surfN[2].x *m_toolN[1].z + m_surfN[1].z *m_toolN[2].x - 2 *m_surfN[2].z *m_toolN[2].x 
			+ 2 *b *(m_surfN[1].z *(m_toolN[1].x - m_toolN[2].x) + m_surfN[2].z *(-m_toolN[1].x + m_toolN[2].x) 
			- (m_surfN[1].x - m_surfN[2].x) *(m_toolN[1].z - m_toolN[2].z)) - m_surfN[1].x *m_toolN[2].z + 2 *m_surfN[2].x *m_toolN[2].z 
			+ a *(-m_surfN[1].x *m_toolN[0].z + m_surfN[2].x *m_toolN[0].z + m_surfN[0].z *m_toolN[1].x - m_surfN[0].x *m_toolN[1].z 
			+ m_surfN[2].x *m_toolN[1].z - m_surfN[2].z *(m_toolN[0].x + m_toolN[1].x - 2 *m_toolN[2].x) 
			+ m_surfN[1].z *(m_toolN[0].x - m_toolN[2].x) - m_surfN[0].z *m_toolN[2].x + m_surfN[0].x *m_toolN[2].z 
			+ m_surfN[1].x *m_toolN[2].z - 2 *m_surfN[2].x *m_toolN[2].z));
	}

	inline double dxfun3(double a, double b) const
	{
		return (-m_surfN[2].y *m_toolN[0].x + m_surfN[2].x *m_toolN[0].y - m_surfN[0].y *m_toolN[2].x + 2 *m_surfN[2].y *m_toolN[2].x 
			+ 2 *a *(m_surfN[2].y *(m_toolN[0].x - m_toolN[2].x) + m_surfN[0].y *(-m_toolN[0].x + m_toolN[2].x) 
			+ (m_surfN[0].x - m_surfN[2].x) *(m_toolN[0].y - m_toolN[2].y)) + m_surfN[0].x *m_toolN[2].y - 2 *m_surfN[2].x *m_toolN[2].y 
			+ b *(m_surfN[1].x *m_toolN[0].y - m_surfN[2].x *m_toolN[0].y - m_surfN[0].y *m_toolN[1].x + m_surfN[0].x *m_toolN[1].y 
			- m_surfN[2].x *m_toolN[1].y + m_surfN[2].y *(m_toolN[0].x + m_toolN[1].x - 2 *m_toolN[2].x) + m_surfN[0].y *m_toolN[2].x 
			+ m_surfN[1].y *(-m_toolN[0].x + m_toolN[2].x) - m_surfN[0].x *m_toolN[2].y - m_surfN[1].x *m_toolN[2].y 
			+ 2 *m_surfN[2].x *m_toolN[2].y));
	}

	inline double dyfun3(double a, double b) const
	{
		return (-m_surfN[2].y *m_toolN[1].x + m_surfN[2].x *m_toolN[1].y - m_surfN[1].y *m_toolN[2].x + 2 *m_surfN[2].y *m_toolN[2].x 
			+ 2 *b *(m_surfN[2].y *(m_toolN[1].x - m_toolN[2].x) + m_surfN[1].y *(-m_toolN[1].x + m_toolN[2].x) 
			+ (m_surfN[1].x - m_surfN[2].x) *(m_toolN[1].y - m_toolN[2].y)) + m_surfN[1].x *m_toolN[2].y - 2 *m_surfN[2].x *m_toolN[2].y 
			+ a *(m_surfN[1].x *m_toolN[0].y - m_surfN[2].x *m_toolN[0].y - m_surfN[0].y *m_toolN[1].x 
			+ m_surfN[0].x *m_toolN[1].y - m_surfN[2].x *m_toolN[1].y + m_surfN[2].y *(m_toolN[0].x + m_toolN[1].x 
			- 2 *m_toolN[2].x) + m_surfN[0].y *m_toolN[2].x + m_surfN[1].y *(-m_toolN[0].x + m_toolN[2].x) - m_surfN[0].x *m_toolN[2].y 
			- m_surfN[1].x *m_toolN[2].y + 2 *m_surfN[2].x *m_toolN[2].y));
	}

	double operator() (double a, double b) const
	{
		MATHLIB::vector3 v;
		v = fun(a,b);
		return v.x * v.x + v.y * v.y + v.z * v.z;
	}

	std::vector<double> Gradient(double a, double b) const
	{
		std::vector<double> res(2);
		MATHLIB::vector3 f = fun(a,b);
		
		res[0] = 2.0 * f.x * dxfun1(a,b)
			+ 2.0 * f.y * dxfun2(a,b)
			+ 2.0 * f.z * dxfun3(a,b);

		res[1] = 2.0 * f.x * dyfun1(a,b)
			+ 2.0 * f.y * dyfun2(a,b)
			+ 2.0 * f.z * dyfun3(a,b);
		return res;
	}
private:
	MATHLIB::vector3 v1, v2, v;
	MATHLIB::matrix33 m_surfN, m_toolN;
};

#endif //STATICPOINTMINIMIZEFUN_H_